#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "ethercat.h"
#include <stdio.h>
#include <time.h>

#ifdef _WIN32
#include <windows.h>
#include <stdint.h>
#include <time.h>
#include <stdio.h>
#include <winsock2.h>
#pragma comment(lib, "ws2_32.lib")
#else
#include <unistd.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <fcntl.h>
#include <errno.h>
#endif

#ifdef _WIN32
#define CLOCK_MONOTONIC 0

#define SLAVE_POSITION 2
#define TARGET_POSITION 100000
#define PROFILE_VELOCITY 10000
#define PROFILE_ACCELERATION 500
#define PROFILE_DECELERATION 500

#define NSEC_PER_SEC 1000000000
#define EC_TIMEOUTMON 500
#define TIMER_ABSTIME 1

#define CYCTIME_MS 20

char IOmap[4096];
uint64_t diff, maxt, avg, cycle;
struct timeval t1, t2;
int dorun = 0;
int64 toff = 0;
int expectedWKC;
boolean needlf;
volatile int wkc;
boolean inOP;
uint8 currentgroup = 0;

enum
{
   STATE_RESET,
   STATE_INIT,
   STATE_PREREADY,
   STATE_READY,
   STATE_ENABLE,
   STATE_DISABLE
};

int state = STATE_RESET;

int clock_gettime(int clk_id, struct timespec *tp)
{
   if (clk_id != CLOCK_MONOTONIC)
   {
      return -1;
   }

   LARGE_INTEGER frequency;
   LARGE_INTEGER time;

   if (!QueryPerformanceFrequency(&frequency))
   {
      return -1;
   }

   if (!QueryPerformanceCounter(&time))
   {
      return -1;
   }

   tp->tv_sec = (long)(time.QuadPart / frequency.QuadPart);
   tp->tv_nsec = (long)((time.QuadPart % frequency.QuadPart) * 1e9 / frequency.QuadPart);

   return 0;
}
#endif

#ifdef _WIN32
#include <windows.h>
#else
#include <unistd.h>
#endif

// TCP Server variables
int tcp_server_socket = -1;
int tcp_client_socket = -1;
fd_set read_fds;

// 全局 target 变量
int target[5] = {0, 0, 0, 0, 0};
int step[5] = {0, 0, 0, 0, 0};

int DI = 0;
int DO = 0;

#define STEP_INIT 0
#define STEP_SET_TARGET 1
#define STEP_RUN 2

// Delay function
void delay_ms(int milliseconds)
{
#ifdef _WIN32
   Sleep(milliseconds);
#else
   usleep(milliseconds * 1000);
#endif
}

void add_timespec(struct timespec *ts, int64 addtime)
{
   long nsec;
   time_t sec;

   nsec = addtime % NSEC_PER_SEC;
   sec = (addtime - nsec) / NSEC_PER_SEC;
   ts->tv_sec += sec;
   ts->tv_nsec += nsec;
   if (ts->tv_nsec > NSEC_PER_SEC)
   {
      nsec = ts->tv_nsec % NSEC_PER_SEC;
      ts->tv_sec += (ts->tv_nsec - nsec) / NSEC_PER_SEC;
      ts->tv_nsec = nsec;
   }
}

void ec_sync(int64 reftime, int64 cycletime, int64 *offsettime)
{
   static int64 integral = 0;
   int64 delta;
   delta = (reftime - 50000) % cycletime;
   if (delta > (cycletime / 2))
   {
      delta = delta - cycletime;
   }
   if (delta > 0)
   {
      integral++;
   }
   if (delta < 0)
   {
      integral--;
   }
   *offsettime = -(delta / 100) - (integral / 20);
}

static int drive_write8(uint16 slave, uint16 index, uint8 subindex, uint8 value)
{
   return ec_SDOwrite(slave, index, subindex, FALSE, sizeof(value), &value, EC_TIMEOUTRXM);
}

static int drive_write16(uint16 slave, uint16 index, uint8 subindex, uint16 value)
{
   return ec_SDOwrite(slave, index, subindex, FALSE, sizeof(value), &value, EC_TIMEOUTRXM);
}

static int drive_write32(uint16 slave, uint16 index, uint8 subindex, int32 value)
{
   return ec_SDOwrite(slave, index, subindex, FALSE, sizeof(value), &value, EC_TIMEOUTRXM);
}

#ifdef _MSC_VER   // Windows MSVC
  #define PACKED
  #pragma pack(push,1)
#else             // GCC/Clang (Linux)
  #define PACKED __attribute__((packed))
#endif
typedef struct PACKED
{
   uint16_t data;
} Drive_Inputs_IO;

typedef struct PACKED
{
   uint16_t data;
} Drive_Outputs_IO;

typedef struct PACKED
{
   uint16_t Controlword;
   uint8_t ModeOp;
   uint32_t PPVel;
   int32_t TargetPos;
   uint32_t PPAC;
   uint32_t PPDC;
   uint8_t padding;
} Drive_Outputs_motor;

typedef struct PACKED
{
   uint16_t Statusword;
   uint8_t ModeOp;
   int32_t ActualPos;
   // uint8_t padding;
} Drive_Inputs_motor;

#ifdef _MSC_VER
  #pragma pack(pop)
#endif

int drive_setup_motor(uint16 slave)
{
   int wkc = 0;

   printf("Drive %d setup\n", slave);

   wkc += drive_write16(slave, 0x1C12, 0, 0);
   wkc += drive_write16(slave, 0x1C13, 0, 0);

   wkc += drive_write8(slave, 0x1A00, 0, 0);
   wkc += drive_write32(slave, 0x1A00, 1, 0x60410010);
   wkc += drive_write32(slave, 0x1A00, 2, 0x60610008);
   wkc += drive_write32(slave, 0x1A00, 3, 0x60640020);
   wkc += drive_write8(slave, 0x1A00, 0, 3);

   wkc += drive_write8(slave, 0x1600, 0, 0);
   wkc += drive_write32(slave, 0x1600, 1, 0x60400010);
   wkc += drive_write32(slave, 0x1600, 2, 0x60600008);
   wkc += drive_write32(slave, 0x1600, 3, 0x60810020);
   wkc += drive_write32(slave, 0x1600, 4, 0x607A0020);
   wkc += drive_write32(slave, 0x1600, 5, 0x60830020);
   wkc += drive_write32(slave, 0x1600, 6, 0x60840020);
   wkc += drive_write8(slave, 0x1600, 0, 6);

   wkc += drive_write16(slave, 0x1C12, 1, 0x1600);
   wkc += drive_write8(slave, 0x1C12, 0, 1);

   wkc += drive_write16(slave, 0x1C13, 1, 0x1A00);
   wkc += drive_write8(slave, 0x1C13, 0, 1);

   strncpy(ec_slave[slave].name, "Drive", EC_MAXNAME);

   if (wkc != 19)
   {
      printf("Drive %d setup failed\nwkc: %d\n", slave, wkc);
      return -1;
   }
   else
      printf("Drive %d setup succeed.\n", slave);

   return 0;
}

// 解析 {target1:20000,target2:30000}
void parse_targets(const char *msg)
{
   // {"target1":40000,"target2":30000,"target3":50000}
   int t1, t2, t3, t4;
   int DO_;
   if (sscanf(msg, "{\"target1\":%d}", &t1) == 1)
   {
      target[0] = t1;
      printf("Updated targets:  target1=%d\n", target[0]);
   }
   else if (sscanf(msg, "{\"target2\":%d}", &t2) == 1)
   {
      target[1] = t2;
      printf("Updated targets:  target2=%d\n", target[1]);
   }
   else if (sscanf(msg, "{\"target3\":%d}", &t3) == 1)
   {
      target[2] = t3;
      printf("Updated targets:  target3=%d\n", target[2]);
   }
   else if (sscanf(msg, "{\"target4\":%d}", &t4) == 1)
   {
      target[3] = t4;
      printf("Updated targets:  target3=%d\n", target[3]);
   }
   else if (sscanf(msg, "{\"DODODODO\":%d}", &DO_) == 1)
   {
      DO = DO_;
      printf("Updated DO:  %d\n", DO);
   }
   else if (sscanf(msg, "{\"target1\":%d,\"target2\":%d,\"target3\":%d}", &t1, &t2, &t3) == 3)
   {
      target[0] = t1;
      target[1] = t2;
      target[2] = t3;
      printf("Updated targets: target1=%d, target2=%d, target3=%d\n", target[0], target[1], target[2]);
   }
   else
   {
      printf("Invalid message format: %s\n", msg);
   }
}

// TCP Server functions
int init_tcp_server()
{
#ifdef _WIN32
   WSADATA wsaData;
   if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0)
   {
      printf("WSAStartup failed\n");
      return -1;
   }
#endif

   tcp_server_socket = socket(AF_INET, SOCK_STREAM, 0);
   if (tcp_server_socket < 0)
   {
      perror("socket");
      return -1;
   }

#ifdef _WIN32
   u_long mode = 1;
   ioctlsocket(tcp_server_socket, FIONBIO, &mode);
#else
   int flags = fcntl(tcp_server_socket, F_GETFL, 0);
   fcntl(tcp_server_socket, F_SETFL, flags | O_NONBLOCK);
#endif

   struct sockaddr_in server_addr;
   memset(&server_addr, 0, sizeof(server_addr));
   server_addr.sin_family = AF_INET;
   server_addr.sin_addr.s_addr = INADDR_ANY;
   server_addr.sin_port = htons(8080);

   if (bind(tcp_server_socket, (struct sockaddr *)&server_addr, sizeof(server_addr)) < 0)
   {
      perror("bind");
      close(tcp_server_socket);
      return -1;
   }

   if (listen(tcp_server_socket, 5) < 0)
   {
      perror("listen");
      close(tcp_server_socket);
      return -1;
   }

   printf("TCP Server started on port 8080\n");
   return 0;
}

void handle_tcp_connections()
{
   if (tcp_client_socket == -1)
   {
      struct sockaddr_in client_addr;
      socklen_t client_len = sizeof(client_addr);

      tcp_client_socket = accept(tcp_server_socket, (struct sockaddr *)&client_addr, &client_len);
      if (tcp_client_socket >= 0)
      {
#ifdef _WIN32
         u_long mode = 1;
         ioctlsocket(tcp_client_socket, FIONBIO, &mode);
#else
         int flags = fcntl(tcp_client_socket, F_GETFL, 0);
         fcntl(tcp_client_socket, F_SETFL, flags | O_NONBLOCK);
#endif
         printf("Client connected: %s:%d\n", inet_ntoa(client_addr.sin_addr), ntohs(client_addr.sin_port));
      }
   }

   if (tcp_client_socket >= 0)
   {
      char buffer[1024];
      int bytes_read;

#ifdef _WIN32
      bytes_read = recv(tcp_client_socket, buffer, sizeof(buffer) - 1, 0);
#else
      bytes_read = read(tcp_client_socket, buffer, sizeof(buffer) - 1);
#endif

      if (bytes_read > 0)
      {
         buffer[bytes_read] = '\0';
         parse_targets(buffer); // 只解析，不回发
      }
      else if (bytes_read == 0)
      {
         printf("Client disconnected\n");
#ifdef _WIN32
         closesocket(tcp_client_socket);
#else
         close(tcp_client_socket);
#endif
         tcp_client_socket = -1;
      }
#ifndef _WIN32
      else if (errno != EWOULDBLOCK && errno != EAGAIN)
      {
         perror("read");
         close(tcp_client_socket);
         tcp_client_socket = -1;
      }
#endif
   }
}

void cleanup_tcp_server()
{
   if (tcp_client_socket >= 0)
   {
#ifdef _WIN32
      closesocket(tcp_client_socket);
#else
      close(tcp_client_socket);
#endif
   }
   if (tcp_server_socket >= 0)
   {
#ifdef _WIN32
      closesocket(tcp_server_socket);
      WSACleanup();
#else
      close(tcp_server_socket);
#endif
   }
}

int GENE(char *ifname)
{
   if (init_tcp_server() != 0)
   {
      printf("Failed to initialize TCP server\n");
      return -1;
   }

   if (!ec_init(ifname))
   {
      printf("EtherCAT initialization failed.\n");
      cleanup_tcp_server();
      return -1;
   }
   printf("EtherCAT initialization succeeded.\n");

   if (ec_config_init(FALSE) <= 0)
   {
      printf("EtherCAT configuration failed.\n");
      ec_close();
      cleanup_tcp_server();
      return -1;
   }
   printf("EtherCAT configuration succeeded.\n");

   int ht;
   int chk = 2000;
   int64 cycletime;
   struct timespec ts;

   for (int slave_ix = 1; slave_ix <= ec_slavecount; slave_ix++)
   {
      ec_slavet *slave = &ec_slave[slave_ix];
      if (slave_ix == 1)
      {
         // slave->PO2SOconfig = drive_setup_IO;
      }
      else
      {
         slave->PO2SOconfig = drive_setup_motor;
      }
   }

   ec_config_map(&IOmap);
   for (int slave_ix = 2; slave_ix <= ec_slavecount; slave_ix++)
   {
      ec_dcsync0(slave_ix, TRUE, CYCTIME_MS * 1000000U, CYCTIME_MS * 1000000U / 2);
   }
   ec_configdc();

   printf("Slaves mapped, state to SAFE_OP.\n");
   ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE);

   ec_readstate();
   for (int slave_ix = 1; slave_ix <= ec_slavecount; slave_ix++)
   {
      printf("Slave:%d Name:%s State:%2d\n",
             slave_ix, ec_slave[slave_ix].name, ec_slave[slave_ix].state);
   }
   expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;
   printf("Calculated workcounter %d\n", expectedWKC);

   ec_statecheck(0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE);
   ec_slave[0].state = EC_STATE_OPERATIONAL;
   ec_writestate(0);

   clock_gettime(CLOCK_MONOTONIC, &ts);
   ht = (ts.tv_nsec / 1000000) + 1;
   ts.tv_nsec = ht * 1000000;
   cycletime = CYCTIME_MS * 1000 * 1000;

   for (int i = 1; i <= ec_slavecount; i++)
   {
      if (i > 1)
      {
         Drive_Outputs_motor *optr = (Drive_Outputs_motor *)ec_slave[i].outputs;
         if (optr)
         {
            optr->Controlword = 0;
            optr->ModeOp = 1;
            optr->TargetPos = 0;
            optr->PPVel = 0;
            optr->PPAC = 0;
            optr->PPDC = 0;
         }
      }
      printf("eep_rev %d : %d \n ", i, ec_slave[i].eep_id);
   }

   do
   {
      osal_usleep(CYCTIME_MS * 1000);
      if (ec_slave[0].hasdc)
      {
         ec_sync(ec_DCtime, cycletime, &toff);
      }
      wkc = ec_receive_processdata(EC_TIMEOUTRET);
      ec_send_processdata();
   } while (chk-- && (wkc != expectedWKC));

   if (wkc == expectedWKC)
   {
      printf("Operational state reached for all slaves.\n");

      osal_usleep((uint32)(cycletime / 1000));
      toff = 0;
      int cnt = 0;

      Drive_Inputs_motor *iptr;
      Drive_Outputs_motor *optr;

      while (1)
      {
         cnt++;
         if (cnt > 100)
            cnt = 90;

         // 处理 TCP 收到的报文
         handle_tcp_connections();

         osal_usleep((uint32)((cycletime + toff) / 1000));

         if (ec_slave[0].hasdc)
         {
            ec_sync(ec_DCtime, cycletime, &toff);
         }
         wkc = ec_receive_processdata(EC_TIMEOUTRET);

         // EtherCAT 数据收发
         for (int slave_ix = 2; slave_ix <= ec_slavecount; slave_ix++)
         {

            optr = (Drive_Outputs_motor *)ec_slave[slave_ix].outputs;
            iptr = (Drive_Inputs_motor *)ec_slave[slave_ix].inputs;

            // printf(" motor %d data: p %ld,t %ld,s 0x%X,c 0x%X,m %d ", slave_ix, iptr->ActualPos, optr->TargetPos, iptr->Statusword, optr->Controlword,iptr->ModeOp );

            // printf("motor %d data: p %ld, v %ld q %d s 0x%X c 0x%X m %d.\n", slave_ix, iptr->ActualPos, iptr->ActualVel, iptr->ActualTor, iptr->Statusword, optr->Controlword,iptr->ModeOp );
            if (cnt == 2)
            {
               optr->Controlword = 128;
            }
            if (cnt == 4)
            {
               optr->ModeOp = 1;
               optr->TargetPos = iptr->ActualPos;
            }
            if (cnt == 4 * 4)
            {
               optr->Controlword = 6;
               optr->TargetPos = iptr->ActualPos;
            }
            if (cnt == 4 * 8)
            {
               optr->Controlword = 7;
               optr->TargetPos = iptr->ActualPos;
               optr->PPAC = 0x8000;
               optr->PPDC = 0x8000;
            }
            if (cnt == 4 * 12)
            {
               optr->Controlword = 0xF;
            }

            if (target[slave_ix - 2] != 0 && ((step[slave_ix - 2] == STEP_INIT) || (step[slave_ix - 2] == STEP_RUN)))
            {
               optr->TargetPos = target[slave_ix - 2];
               optr->PPVel = (uint32_t)(PROFILE_VELOCITY * 1.5);
               optr->Controlword = 0xF;
               printf("target%d 0F %d \n", slave_ix, target[slave_ix - 2]);
               step[slave_ix - 2] = STEP_SET_TARGET;
            }
            else if (target[slave_ix - 2] != 0 && (step[slave_ix - 2] == STEP_SET_TARGET))
            {
               optr->Controlword = 0x1F;
               printf("target%d 1F %d \n", slave_ix, target[slave_ix - 2]);
               step[slave_ix - 2] = STEP_RUN;
               target[slave_ix - 2] = 0;
            }
         }

         ((Drive_Outputs_IO *)ec_slave[1].outputs)->data = DO;

         char json_buf[512];
         int written = 0;

         // 开始JSON对象
         written = snprintf(json_buf, sizeof(json_buf), "{\"motors\":[");
         // 循环添加电机数据 (从站2到ec_slavecount)
         for (int slave_ix = 2; slave_ix <= ec_slavecount; slave_ix++)
         {
            Drive_Inputs_motor *motor_inputs = (Drive_Inputs_motor *)ec_slave[slave_ix].inputs;

            // 添加逗号分隔（除了第一个元素）
            if (slave_ix > 2)
            {
               written += snprintf(json_buf + written, sizeof(json_buf) - written, ",");
            }

            // 添加单个电机数据
            written += snprintf(json_buf + written, sizeof(json_buf) - written,
                                "{\"position\":%ld,\"status\":%d}",
                                motor_inputs->ActualPos,
                                motor_inputs->Statusword);
         }

         // 添加IO数据
         Drive_Inputs_IO *io_inputs = (Drive_Inputs_IO *)ec_slave[1].inputs;
         written += snprintf(json_buf + written, sizeof(json_buf) - written,
                             "],\"IOs\":[{\"DI\":%d,\"DO\":%d}]}\n",
                             io_inputs->data, DO);

         if (tcp_client_socket >= 0)
         {
#ifdef _WIN32
            if (cnt == 90)
            {
               send(tcp_client_socket, json_buf, strlen(json_buf), 0);
            }
#else
            write(tcp_client_socket, json_buf, strlen(json_buf));
#endif
         }

         ec_send_processdata();
      }
   }
   else
   {
      printf("Not all slaves reached operational state.\n");
   }

   ec_close();
   cleanup_tcp_server();
   return 0;
}

char ifbuf[1024];

int main(int argc, char *argv[])
{
   ec_adaptert *adapter = NULL;
   printf(" PDO JiHua \n");

   if (argc > 1)
   {
      strcpy(ifbuf, argv[1]);
      GENE(ifbuf);
   }
   else
   {
      printf("Usage: slaveinfo ifname [options]\nifname = eth0 for example\n");
      printf("Available adapters\n");
      adapter = ec_find_adapters();
      while (adapter != NULL)
      {
         printf("Description : %s, Device to use for wpcap: %s\n", adapter->desc, adapter->name);
         adapter = adapter->next;
      }
   }

   printf("End program\n");
   return 0;
}
